/*
The surface-physics project: http:// code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy 
License : Apache License 2.0

Bump_CollisionShape.cpp
Bump_CollisionShape management implementation.


*/


/* Class declaration */
#include "Bump_CollisionShape.h"


/**
 * Constructor for this class
 *
 */
Bump_CollisionShape::Bump_CollisionShape()
{
	bumpBase = NULL;
	mShapeSet = false;
	mNumShapes = 0;
	strcpy(strMeshVesselName, "MeshProxy");

}


/**
 * Destructor, animations are already deleted when its called
 * so may need better way to delete animation midsim
 */
Bump_CollisionShape::~Bump_CollisionShape()
{
	removeShape();
	//oapiWriteLogV("Bump_CollisionShape::exit: Destroyed CollisionShape : %s",
	//						strMeshVesselName);

}


/**
 * Updates a base mesh
 * Primarily move the mesh into position according to latest transform by setting
 * translation, yaw ,pitch & roll using animation handles created in init().
 *
 * This function assumes the shape is set.
 *
 */
int
Bump_CollisionShape::drawSimpleShape(btTransform& transShape)
{
	if (!mShapeSet)
		return 1;

	int i = 0;
	// Translation values
	float tx, ty, tz;

	// Rotation Euler angles
	float rx, ry, rz;



	btVector3 origin = transShape.getOrigin();
	tx = origin.x();
	ty = origin.y();
	tz = origin.z();
	transShape.getBasis().getEulerYPR(rz, ry, rx);

	/*if (numShapes > 1)
		oapiWriteLogV("translation: %f, %f, %f, rx=%f, ry=%f, rz=%f\n",
				tx, ty, tz, rx*180/PI, ry*180/PI, rz*180/PI);*/



	double state = 0.0f;

	// Translate
	state = fabs(tx/Bump_BulletBase::BULLET_WORLD_SIZE);
	state *= 0.5; 								  //reduce range to within  0 to 0.5 or 0.5 to 1
	state = (tx > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
	bumpBase->bb->SetAnimation(animTX[i], state);

	state = fabs(ty/Bump_BulletBase::BULLET_WORLD_SIZE);
	state *= 0.5;
	state = (ty > 0) ? 0.5+state : 0.5-state ;
	bumpBase->bb->SetAnimation(animTY[i], state);

	state = fabs(tz/Bump_BulletBase::BULLET_WORLD_SIZE);
	state *= 0.5; //reduce range to within  0 to 0.5 or 0.5 to 1
	state = (tz > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
	bumpBase->bb->SetAnimation(animTZ[i], state);



	// Rotate
	state = fabs((rz+PI)/(2*PI) );
	bumpBase->bb->SetAnimation(animRZ[i], state);

	state = fabs((-ry+PI)/(2*PI)); //Very important to negate RY
	bumpBase->bb->SetAnimation(animRY[i], state);

	state = fabs((rx+PI)/(2*PI));
	bumpBase->bb->SetAnimation(animRX[i], state);


/*		if (i > 1)
		sprintf(oapiDebugString(), "translation: %f, %f, %f, rx=%f, ry=%f, rz=%f\n",
				tx[i], ty[i], tz[i], rx[i]*180/PI, ry[i]*180/PI, rz[i]*180/PI);*/

	// Scale
	bumpBase->bb->SetAnimation(animSX[i], 1);
	bumpBase->bb->SetAnimation(animSY[i], 1);
	bumpBase->bb->SetAnimation(animSZ[i], 1);



	return 0;
}


/**
 * Updates local transforms of compound shapes - called only local
 * transforms have changed in object space.
 */
int
Bump_CollisionShape::drawCompoundShape(btTransform& transShape, btCompoundShape& compoundShape)
{
	if (!mShapeSet)
		return 1;

	// Translation values
	float tx, ty, tz;

	// Rotation Euler angles
	float rx, ry, rz;



	for (int i = 0; i < mNumShapes; ++i) {


		// Get the current local transformations
		btTransform transChild = compoundShape.getChildTransform(i);

		// Get the child world space transforms
		btTransform transWorldSpace = transShape * transChild;

		// Extract the translations/ rotation angles for the children
		btVector3 origin = transWorldSpace.getOrigin();
		tx = origin.x();
		ty = origin.y();
		tz = origin.z();
		transWorldSpace.getBasis().getEulerYPR(rz, ry, rx);



		/*if (i == 3 && strcmp(strMeshVesselName, "Collider1_Chassis") == 0) {
			oapiWriteLogV("translation: %f, %f, %f, rx=%f, ry=%f, rz=%f\n",
								tx, ty, tz, rx*180/PI, ry*180/PI, rz*180/PI);
		}*/

		double state = 0.0f;

		// Translate
		state = fabs(tx/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5; 								  //reduce range to within  0 to 0.5 or 0.5 to 1
		state = (tx > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
		bumpBase->bb->SetAnimation(animTX[i], state);

		state = fabs(ty/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5;
		state = (ty > 0) ? 0.5+state : 0.5-state ;
		bumpBase->bb->SetAnimation(animTY[i], state);

		state = fabs(tz/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5; //reduce range to within  0 to 0.5 or 0.5 to 1
		state = (tz > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
		bumpBase->bb->SetAnimation(animTZ[i], state);



		// Rotate
		state = fabs((rz+PI)/(2*PI) );
		bumpBase->bb->SetAnimation(animRZ[i], state);

		state = fabs((-ry+PI)/(2*PI)); //Very important to negate RY
		bumpBase->bb->SetAnimation(animRY[i], state);

		state = fabs((rx+PI)/(2*PI));
		bumpBase->bb->SetAnimation(animRX[i], state);



		/*sprintf(oapiDebugString(), "translation: %f, %f, %f, rx=%f, ry=%f, rz=%f\n",
					tx, ty, tz, rx*180/PI, ry*180/PI, rz*180/PI);*/

		// Scale
		bumpBase->bb->SetAnimation(animSX[i], 1);
		bumpBase->bb->SetAnimation(animSY[i], 1);
		bumpBase->bb->SetAnimation(animSZ[i], 1);



	}

	return 0;
}


/**
 * Creates the animation handles with the rotations around the axes as child anims
 * of the translations.
 *
 * It uses Bump_BulletBase::BULLET_WORLD_SIZE as the maximum size of the BulletBase VESSEL
 * Reads meshes from ROOT/Meshes/Bump/CollisionMeshes/
 *
 */
int
Bump_CollisionShape::addSimpleShape(Bump_BulletBase *base,
							  	  	btCollisionShape *collShape,
							  	  	char *strMshVesselName,
							  	  	char *fileName)
{
	bumpBase = base;
	strcpy(strMeshVesselName, strMshVesselName); //DO NOT REMOVE THIS : this is the MESH VESSEL's name


	// Load the mesh
	VECTOR3 ofs = _V(0, 0, 0);


	if (collShape->getShapeType() == BOX_SHAPE_PROXYTYPE) {           //collision box
		const btBoxShape* boxShape = static_cast<const btBoxShape*>(collShape);
		btVector3 halfExtent = boxShape->getHalfExtentsWithoutMargin();

		strcpy(strMeshFileName, "Bump/CollisionMeshes/UnitCube");
		mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(strMeshFileName, &ofs);

		createAnimations(2 * halfExtent.x(),
						 2 * halfExtent.y(),
						 2 * halfExtent.z());
	}
	else if (collShape->getShapeType() == SPHERE_SHAPE_PROXYTYPE) {

		const char *strCylName = static_cast<const btSphereShape*>(collShape)->getName();
		btScalar sphRadius = static_cast<const btSphereShape*>(collShape)->getRadius();

		strcpy(strMeshFileName, "Bump/CollisionMeshes/UnitSphere");
		mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(strMeshFileName, &ofs);

		createAnimations(sphRadius,
						 sphRadius,
						 sphRadius);

	}
	else if (collShape->getShapeType() == CYLINDER_SHAPE_PROXYTYPE) {

		const char *strCylName = static_cast<const btCylinderShape*>(collShape)->getName();
		btVector3 halfExtent = static_cast<const btCylinderShape*>(collShape)->getHalfExtentsWithoutMargin();

		if (strcmp(strCylName, "CylinderX") == 0) {

			strcpy(strMeshFileName, "Bump/CollisionMeshes/UnitCylinderX");

			mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(strMeshFileName, &ofs);			

			createAnimations(2 * halfExtent.x(),  // ht
								 halfExtent.y(),  // rad
								 halfExtent.z()); // rad

		}
		else if (strcmp(strCylName, "CylinderY") == 0) {

			strcpy(strMeshFileName, "Bump/CollisionMeshes/UnitCylinderY");
			mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(strMeshFileName, &ofs);

			createAnimations(    halfExtent.x(),  // rad
							 2 * halfExtent.y(),  // ht
								 halfExtent.z()); // rad


		}
		else {	// CylinderZ
			strcpy(strMeshFileName, "Bump/CollisionMeshes/UnitCylinderZ");
			mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(strMeshFileName, &ofs);

			createAnimations(    halfExtent.x(),  // rad
							     halfExtent.y(),  // rad
							 2 * halfExtent.z()); // ht
		}
	}
	else if (collShape->getShapeType() == CONE_SHAPE_PROXYTYPE) {

		float radius = static_cast<const btConeShape*>(collShape)->getRadius();
		float height = static_cast<const btConeShape*>(collShape)->getHeight();

		if (static_cast<const btConeShape*>(collShape)->getConeUpIndex() == 0) {

			strcpy(strMeshFileName, "Bump/CollisionMeshes/UnitConeX");
			mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(strMeshFileName, &ofs);

			createAnimations(height,  radius, radius);
		}
		else if (static_cast<const btConeShape*>(collShape)->getConeUpIndex() == 1) {

			strcpy(strMeshFileName, "Bump/CollisionMeshes/UnitConeY");
			mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(strMeshFileName, &ofs);

			createAnimations(radius, height, radius);
		}
		else {	// ConeZ
			strcpy(strMeshFileName, "Bump/CollisionMeshes/UnitConeZ");
			mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(strMeshFileName, &ofs);

			createAnimations(radius, radius, height);
		}
	}
	else if (collShape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE) {

		/*char *pFileName = (char*)collShape->getUserPointer();
		oapiWriteLogV("Bump_CollisionShape::addSimpleShape: Convex Hull File Name: %s", pFileName);*/

		if (fileName) {
			mMeshIndices[mNumShapes] = bumpBase->bb->AddMesh(fileName, &ofs);
			createAnimations(1, 1, 1);
		}
		else
			oapiWriteLogV("Bump_CollisionShape::addSimpleShape: ERROR No filename\
					specified for Convex Hull");


	}
	else {
		oapiWriteLogV("Bump_CollisionShape::addSimpleShape: ERROR : Invalid shape passed %d for %s",
						collShape->getShapeType(), strMeshVesselName);
		return 1;
	}

	++mNumShapes;
	mShapeSet = true; // set multiple times for a compound shape

	return 0;
}


/**
 * Add a compound shape
 */
int
Bump_CollisionShape::addCompoundShape(Bump_BulletBase *base,
									  btCollisionShape *collShape,
									  char *strMshVesselName,
									  char *pFileNames[])
{

	if (mShapeSet) {
		oapiWriteLogV("Bump_CollisionShape::addCompoundShape: ERROR Shape has already been set with\
				%d shapes", mNumShapes);
		return 1;
	}



	int rv = 0;

	bumpBase = base;
	strcpy(strMeshVesselName, strMshVesselName); //DO NOT REMOVE THIS : this is the MESH VESSEL's name

	btCompoundShape *compoundShape = (btCompoundShape*)collShape;

	int numShapes = compoundShape->getNumChildShapes();

	if (numShapes > VesselCollisionDef::MAX_SHAPES) {		
		oapiWriteLogV("Bump_CollisionShape::addCompoundShape: WARN Too many shapes in \
				 compound shape (%d), adding only %d shapes", numShapes, VesselCollisionDef::MAX_SHAPES);
		numShapes = VesselCollisionDef::MAX_SHAPES;
	}

	for (int i = 0; i < numShapes; ++i) {
		btCollisionShape *pShape = compoundShape->getChildShape(i);
		rv = addSimpleShape(base, pShape, strMshVesselName, pFileNames[i]);
		if (rv) {
			oapiWriteLogV("Bump_CollisionShape::addCompoundShape: ERROR while adding shape %d : %s",
							i, pShape->getName());
			return rv;
		}


		/*oapiWriteLogV("Bump_CollisionShape::addCompoundShape: Added shape %d : %s",
				i, pShape->getName());*/
	}

	return 0;
}


/**
 * Create the scaling animations
 */
int
Bump_CollisionShape::createAnimations(float sx, float sy, float sz)
{
	sx = fabs(sx);
	sy = fabs(sy);
	sz = fabs(sz);

	group[mNumShapes][0] = 0;

	// Translation --------------------------
	mgtTX[mNumShapes] = new MGROUP_TRANSLATE( mMeshIndices[mNumShapes], group[mNumShapes], 0, _V(2*Bump_BulletBase::BULLET_WORLD_SIZE, 0, 0));
	animTX[mNumShapes] = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTX[mNumShapes], 0, 1, mgtTX[mNumShapes]);

	mgtTY[mNumShapes] = new MGROUP_TRANSLATE( mMeshIndices[mNumShapes], group[mNumShapes], 0, _V(0, 2*Bump_BulletBase::BULLET_WORLD_SIZE, 0));
	animTY[mNumShapes] = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTY[mNumShapes], 0, 1, mgtTY[mNumShapes], parent);

	mgtTZ[mNumShapes] = new MGROUP_TRANSLATE( mMeshIndices[mNumShapes], group[mNumShapes], 0, _V(0, 0, 2*Bump_BulletBase::BULLET_WORLD_SIZE));
	animTZ[mNumShapes] = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTZ[mNumShapes], 0, 1, mgtTZ[mNumShapes], parent);


	// Scaling ------------------------------
	mgsSX[mNumShapes] =  new MGROUP_SCALE( mMeshIndices[mNumShapes], group[mNumShapes], 0, _V(0, 0, 0), _V(sx, 1, 1));
	animSX[mNumShapes] = bumpBase->bb->CreateAnimation (0);
	parent = bumpBase->bb->AddAnimationComponent (animSX[mNumShapes], 0, 1, mgsSX[mNumShapes], parent);

	mgsSY[mNumShapes] =  new MGROUP_SCALE( mMeshIndices[mNumShapes], group[mNumShapes], 0, _V(0, 0, 0), _V(1, sy, 1));
	animSY[mNumShapes] = bumpBase->bb->CreateAnimation (0);
	parent = bumpBase->bb->AddAnimationComponent (animSY[mNumShapes], 0, 1, mgsSY[mNumShapes], parent);

	// Note the 1 after group
	mgsSZ[mNumShapes] =  new MGROUP_SCALE( mMeshIndices[mNumShapes], group[mNumShapes], 0, _V(0, 0, 0), _V(1, 1, sz));
	animSZ[mNumShapes] = bumpBase->bb->CreateAnimation (0);
	parent = bumpBase->bb->AddAnimationComponent (animSZ[mNumShapes], 0, 1, mgsSZ[mNumShapes], parent);


	// Rotation ----------------------------
	mgrRZ[mNumShapes] = new MGROUP_ROTATE( mMeshIndices[mNumShapes], group[mNumShapes], 0, _V(0, 0, 0), _V(0, 0, 1), (float)(2 * PI) );
	animRZ[mNumShapes] = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRZ[mNumShapes], 0, 1, mgrRZ[mNumShapes], parent);

	mgrRY[mNumShapes] = new MGROUP_ROTATE( mMeshIndices[mNumShapes], group[mNumShapes], 0, _V(0, 0, 0), _V(0, 1, 0), (float)(2 * PI) );
	animRY[mNumShapes] = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRY[mNumShapes], 0, 1, mgrRY[mNumShapes], parent);

	mgrRX[mNumShapes] = new MGROUP_ROTATE( mMeshIndices[mNumShapes], group[mNumShapes], 1, _V(0, 0, 0), _V(1, 0, 0), (float)(2 * PI) );
	animRX[mNumShapes] = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRX[mNumShapes], 0, 1, mgrRX[mNumShapes], parent);


	return 0;
}


/**
 * Remove shape, independent flags for shape set, vessel attached, vessel created
 */
int
Bump_CollisionShape::removeShape(void)
{
	//oapiWriteLogV("Bump_CollisionShape::removeShape : called for %s", strMeshVesselName);


	// Free the animation mesh group structures
	for (int i = 0; i < mNumShapes; ++i) {

		//oapiWriteLogV("Bump_CollisionShape::removeShape : Deleting %p, %p, %p, idx: %d, numShapes:%d",
		//		mgsSX[i], mgsSY[i], mgsSZ[i], mMeshIndices[i], mNumShapes);

		// Clear added meshes
		if (! bumpBase->simIsEnding)
			bumpBase->bb->DelMesh(mMeshIndices[i]);


		// Do not use DelAnimation to delete the animations, they are deleted when their mesh is deleted

		// These deletes are needed, they are not deleted by DelMesh or by Orbiter at end of animation
		// Translation
		delete mgtTX[i];
		delete mgtTY[i];
		delete mgtTZ[i];


		// Rotation
		delete mgrRX[i];
		delete mgrRY[i];
		delete mgrRZ[i];

		// Scaling		
		delete mgsSX[i];
		delete mgsSY[i];
		delete mgsSZ[i];

	}

	mShapeSet = false;

	//oapiWriteLogV("Bump_CollisionShape::removeShape : Removed Mesh VESSEL %s", strMeshVesselName);

	return 0;

}


/**
 * Reset animations, not used currently
 */
int
Bump_CollisionShape::resetAnimations(void)
{
	oapiWriteLogV("Bump_CollisionShape::resetAnimations : called for %s", strMeshVesselName);


	// Free the animation mesh group structures
	for (int i = 0; i < mNumShapes; ++i) {

		double state = 0.5;

		bumpBase->bb->SetAnimation(animTX[i], state);
		bumpBase->bb->SetAnimation(animTY[i], state);
		bumpBase->bb->SetAnimation(animTZ[i], state);

		bumpBase->bb->SetAnimation(animSX[i], state);
		bumpBase->bb->SetAnimation(animSY[i], state);
		bumpBase->bb->SetAnimation(animSZ[i], state);

		bumpBase->bb->SetAnimation(animRX[i], state);
		bumpBase->bb->SetAnimation(animRY[i], state);
		bumpBase->bb->SetAnimation(animRZ[i], state);

	}


	oapiWriteLogV("Bump_CollisionShape::resetAnimations : Reset animations for VESSEL %s", strMeshVesselName);

	return 0;

}


/**
 * Create a Orbiter mesh that represents a box and add to the
 * Orbiter collision mesh
 *
 * This is currently just a place holder, we decided not to generate the meshes
 * manually. We will just scale unit meshes. Convex hulls will use the mesh
 * file name.
 *
 * When we move on to a proper gfx engine we will render the shapes properly
 * Else generating meshes each time they are added/removed will be slow.
 *
 */
MESHHANDLE
Bump_CollisionShape::createBoxShape(btVector3& halfExtent)
{

/*	vertices[0].x = 0;
	vertices[0].y = 0;
	vertices[0].z = 0;
	vertices[0].nx = 0.0;
	vertices[0].ny = -1.0;
	vertices[0].nz = 0.0;

	vertices[1].x = 100;
	vertices[1].y = 100;
	vertices[1].z = 0;
	vertices[1].nx = 0.0;
	vertices[1].ny = -1.0;
	vertices[1].nz = 0.0;

	vertices[2].x = 100;
	vertices[2].y = 0;
	vertices[2].z = 0;
	vertices[2].nx = 0.0;
	vertices[2].ny = -1.0;
	vertices[2].nz = 0.0;



	indices[0] = 0;
	indices[1] = 1;
	indices[2] = 2;


	grp->Vtx = vertices;
	grp->Idx = indices;
	grp->nVtx = 3;
	grp->nIdx = 3;
*/


	return 0;
}
